'''
Created on Aug 6, 2013

@author: csiebern
'''
import numpy as np
import pyodbc as odbc
import os

def toGPSss(HHMMSS):
    HH = np.floor(HHMMSS/10000)
    MM = np.floor((HHMMSS - 10000*HH)/100)
    SS = HHMMSS - 10000*HH - 100*MM
    return 3600.0*HH + 60.0*MM + SS

def toGPSHHMMSS(SS):
    HH = np.floor(SS/3600)
    MM = np.floor((SS - 3600*HH)/60)
    SS = SS - 3600*HH - 60*MM
    return 10000*HH + 100*MM + SS

def toAzimuth(GPSHeading):
    Az = 90 - GPSHeading*(180)/np.pi
    if Az < 0:
        Az = Az + 360
    return Az
    
def M3(heading):
    h = heading*np.pi/180.0
    cost = np.cos(h)
    sint = np.sin(h)
    return np.array([[ cost, sint, 0, 0],
                     [-sint, cost, 0, 0],
                     [    0,    0, 1, 0],
                     [    0,    0, 0, 1]])

def M2(roll):
    r = roll*np.pi/180.0
    cosr = np.cos(r)
    sinr = np.sin(r)
    return np.array([[cosr, 0, -sinr, 0],
                     [   0, 1,     0, 0],
                     [sinr, 0,  cosr, 0],
                     [   0, 0,     0, 1]])

def M1(pitch):
    p = pitch*np.pi/180.0
    cosp = np.cos(p)
    sinp = np.sin(p)
    return np.array([[1,    0,    0, 0],
                     [0, cosp, sinp, 0],
                     [0,-sinp, cosp, 0],
                     [0,    0,    0, 1]])

def Txyz(x, y, z):
    return np.array([[1,0,0,x],
                     [0,1,0,y],
                     [0,0,1,z],
                     [0,0,0,1]])
    
def tfmECEF(obs, sensor):
        
    # Convert LIDAR xyz obs to Homogeneous form
    (r,c) = obs.shape  # @UnusedVariable
    pad   = np.ones((1,c))
    Lxyz  = np.vstack((obs[1:4,:],pad))
        
    # Transform to platform frame (Using Lever Arms and orientations)
    S1 = np.dot(M1(-sensor.pitch),Lxyz)
    S2 = np.dot(M2(-sensor.roll),S1)
    S3 = np.dot(M3(-sensor.heading),S2)
    Pxyz = np.dot(Txyz(-sensor.dX,-sensor.dY,-sensor.dZ),S3)
          
    # Transform platform to ECEF Frame using Trajectory Orientation        
    Xecef = np.ones(Pxyz.shape)
     
    # Assume WGS-84 projection
    a = 6378137
    f = 1.0/298.257223563
     
    for point in range(c):
        X   = Pxyz[:,point]
        lat = obs[4,point]*np.pi/180.0
        lon = obs[5,point]*np.pi/180.0
        alt = obs[6,point]
        h   = obs[7,point]
        r   = obs[8,point]
        p   = obs[9,point]
         
        # convert position to ECEF
        vh = a / (1 - f*(2-f)*np.sin(lat)**2)**0.5         
        x  = (vh + alt)*np.cos(lat)*np.cos(lon)
        y  = (vh + alt)*np.cos(lat)*np.sin(lon)
        z  = (vh*(1 - f*(2-f)) + alt)*np.sin(lat)
        
        #Transform point from platform to ENU(roll, pitch, heading)
        X = np.dot(M1(p),X)
        X = np.dot(M2(r),X)
        X = np.dot(M3(h),X)
        X = X[0:3]
  
        #Transform from ENU to ECEF
        # See www.epsg.org/guides/docs/g7-2.pdf page 97 for formula
        sinlon = np.sin(lon)
        coslon = np.cos(lon)
        sinlat = np.sin(lat)
        coslat = np.cos(lat)
        
        R = np.array([[-sinlon,        -coslon,             0],
                      [-sinlat*coslon, -sinlat*sinlon, coslat],
                      [coslat*coslon,   coslat*sinlon, sinlat]])
        Rinv = np.linalg.inv(R)
        #R is orthogonal so R transpose = R inverse, however precision issues so using inverse...
        ECEF = np.dot(Rinv,X) + np.array([x,y,z])
     
        Xecef[0,point] = ECEF[0]
        Xecef[1,point] = ECEF[1]
        Xecef[2,point] = ECEF[2]
     
    return Pxyz, X, Xecef
    
class Traj:
    def __init__(self,GPSfile):
        fmtGPS = np.dtype({'GPSTime'  :('<f8',   0), # IN GPS HHMMSS of day
                           'Latitude' :('<f8',  40),
                           'Longitude':('<f8',  48),
                           'Altitude' :('<f8',  80),
                           'Heading'  :('<f8', 136),
                           'Roll'     :('<f8', 144),
                           'Pitch'    :('<f8', 152),
                           'JUNK'     :('152V',160)})
        
        self.data = np.memmap(GPSfile, dtype=fmtGPS, mode='r', offset=85)
        
        # Calculate the total Number of Records in a GPS file
        self.records = self.data.size
        
        # Retrieve the first GPS time stamp in the file (HHMMSS.SS)
        self.start = toGPSss(self.data[0][0])
        
        # Retrieve the last GPS time stamp in the file (HHMMSS.SS)
        self.end = toGPSss(self.data[-1][0])
        
        # Calculate the IMU data rate of the GPS file
        self.rate = (self.end - self.start)/(self.records - 1)
        self.rate = np.round(self.rate, 4)
        
        # Define ClipID attribute
        FileName = os.path.basename(GPSfile)
        FileName = os.path.splitext(FileName)
        self.clipID = FileName[0]
        
    def getState(self, tSS):
        t     = toGPSHHMMSS(tSS)
        
        lat   = np.interp(t,self.data['GPSTime'],self.data['Latitude'])
        lon   = np.interp(t,self.data['GPSTime'],self.data['Longitude'])
        alt   = np.interp(t,self.data['GPSTime'],self.data['Altitude'])
        head  = np.interp(t,self.data['GPSTime'],self.data['Heading'])
        roll  = np.interp(t,self.data['GPSTime'],self.data['Roll'])
        pitch = np.interp(t,self.data['GPSTime'],self.data['Pitch'])
        
        return np.vstack((lat,lon,alt,head,roll,pitch))
    
class LiDAR:
    def __init__(self, LiDARfile):
        fmtLAS20Header = np.dtype({'FileSig'    :('4S',      0),
                                   'FileId'     :('<u4',     4),
                                   'GUID1'      :('<u4',     8),
                                   'GUID2'      :('<u2',    12),
                                   'GUID3'      :('<u2',    14),
                                    'GUID4'      :('8S',     16),
                                    'VerMaj'     :('<u1',    24),
                                    'VerMin'     :('<u1',    25),
                                    'SysID'      :('32S',    26),
                                    'GenSoft'    :('32S',    58),
                                    'FileDate'   :('<u4',    90),
                                    'FileTime'   :('<u4',    94),
                                    'HeaderSize' :('<u2',    98),
                                    'SrcIDPacket':('<u2',   100),
                                    'PntMetaOfs' :('<u2',   102),
                                    'PntMetaLen' :('<u2',   104),
                                    'PntRecLen'  :('<u2',   106),
                                    'PntRecOfs'  :('<u8',   108),
                                    'PntCount'   :('<u8',   116),
                                    'VLROfs'     :('<u4',   124),
                                    'VLRCount'   :('<u4',   128),
                                    'PntCntRet'  :('16<u4', 132),
                                    'PntRecComp' :('<u1',   196),
                                    'CRSType'    :('<u1',   197),
                                    'XYUnits'    :('<u1',   198),
                                    'ZUnits'     :('<u1',   199),
                                    'OriginX'    :('<f8',   200),
                                    'OriginY'    :('<f8',   208),
                                    'OriginZ'    :('<f8',   216),
                                    'ScaleBool'  :('<b1',   224),
                                    'ScaleX'     :('<f8',   225),
                                    'ScaleY'     :('<f8',   233),
                                    'ScaleZ'     :('<f8',   241),
                                    'OffsetBool' :('<b1',   249),
                                    'OffsetX'    :('<f8',   250),
                                    'OffsetY'    :('<f8',   258),
                                    'OffsetZ'    :('<f8',   266),
                                    'MAXX'       :('<f8',   274),
                                    'MINX'       :('<f8',   282),
                                    'MAXY'       :('<f8',   290),
                                    'MINY'       :('<f8',   298),
                                    'MAXZ'       :('<f8',   306),
                                    'MINZ'       :('<f8',   314)})
        
        fmtTrimbleLiDAR = np.dtype({'LASF_X'       :('<f8',  0),
                                    'LASF_Y'       :('<f8',  8),
                                    'LASF_Z'       :('<f8', 16),
                                    'LASF_I'       :('<f4', 24),
                                    'LASF_Class'   :('<u1', 28),
                                    'LASF_PntSrcID':('<u2', 29),
                                    'JUNK'         :('<V1', 31),
                                    'G3D_LATRANGE' :('<f4', 32),
                                    'G3D_RED'      :('<u1', 36),
                                    'G3D_GREEN'    :('<u1', 37),
                                    'G3D_BLUE'     :('<u1', 38),
                                    'G3D_ClassConf':('<u1', 39),
# Revised 8/9/2013
# Reversed the H and V angles from the pointrecord metatdata to conform to the Riegl convention.
                                    'G3D_AngleV'   :('<f4', 40),
                                    'G3D_AngleH'   :('<f4', 44),
# End Revision
                                    'G3D_DISTANCE' :('<f4', 48),
                                    'G3D_Deviation':('<f4', 52),
                                    'G3D_SCANSIZE' :('<u4', 56),
                                    'G3D_GPSTIME'  :('<f8', 60),
                                    'G3D_TT'       :('<f8', 68),
                                    'G3D_DELTAPPS' :('<f8', 76),
                                    'G3D_DELTASYNC':('<f8', 84)})
        
        self.header  = np.fromfile(LiDARfile, dtype=fmtLAS20Header, count=1)
        self.data    = np.memmap(LiDARfile, dtype=fmtTrimbleLiDAR, mode='r', offset=self.header['PntRecOfs'])
        self.records = self.data.size
        self.GPStime = self.data['G3D_GPSTIME']

class sensor:
    def __init__(self, row, sensorpath):
        self.dX = row.ORI2SUP_DX
        self.dY = row.ORI2SUP_DY
        self.dZ = row.ORI2SUP_DZ
        self.heading = row.Heading
        self.roll = row.Roll
        self.pitch = row.Pitch
        self.GPSlag = row.GPSLag
        self.datapath = sensorpath
        
    def getOBS(self, clip, start=0, count=1800):
        # find the LAS file corresponding to the clip by name
        for root, dirs, files in os.walk(self.datapath):  # @UnusedVariable
            filename = [name for name in files if clip.clipID in name] 
            if not filename:
                print "No LIDAR file associated with CLIP"
            else:
                filename = os.path.join(root, name)
                lasfile = LiDAR(filename)
                                
                # calculate the lower and upper index
                l = start
                h = start + count - 1

                # Create observation data array
                r  = lasfile.data['G3D_DISTANCE'][l:h]
                theta  = lasfile.data['G3D_AngleH'][l:h]
                phi    = lasfile.data['G3D_AngleV'][l:h]            
                t  = lasfile.data['G3D_GPSTIME'][l:h] # Point Return time in SS of Day

                X = r*np.cos(theta)*np.sin(phi)
                Y = r*np.sin(theta)*np.sin(phi)

                Q1 = phi < np.pi/2 
                Q1 = -1*Q1.astype(int) #assign all Q1 scalar -1, all Q2 scalar 0
                Q2 = Q1 + 1            #assign all Q1 scalar  0, all Q2 scalar 1

                Z1 = Q1*r*np.cos(phi) # multiply all Q1 by -1, rest by 0
                Z2 = Q2*r*np.cos(phi) # multiply all Q2 by  1, rest by 0
                Z  = Z1 + Z2
                   
                # Interpolate position and orientation information
                gps = clip.getState(t)

                return np.vstack((t,X,Y,Z,gps))
        
    def getPOS(self, clip, start=0, count=1800):
        # find the LAS file corresponding to the clip by name
        for root, dirs, files in os.walk(self.datapath):  # @UnusedVariable
            filename = [name for name in files if clip.clipID in name] 
            if not filename:
                print "No LIDAR file associated with CLIP"
            else:
                filename = os.path.join(root, name)
                lasfile = LiDAR(filename)
                                
                # calculate the lower and upper index
                l = start
                h = start + count - 1
        
                # Create position data array
                x = lasfile.data['LASF_X'][l:h]
                y = lasfile.data['LASF_Y'][l:h]
                z = lasfile.data['LASF_Z'][l:h]
        
                return np.vstack((x,y,z))
        
class T3DProj:
    def __init__(self, mdb, drive):
        # Create an ODBC connection
        db = odbc.connect(DSN=mdb)
        cursor = db.cursor()
        cursor2 = db.cursor()
        
        #Create an iterable run list
        runs = cursor.execute("select RunName, Path from Run")
        
        # initialize empty clips attribute
        self.clips = []
        self.ProjPath = ''
        for row in runs:            
            # capture the file path
            if self.ProjPath == '':
                path = row.Path.split()
                path = path[0].replace('\x00','')
                path = path.replace("\\", "/")
                self.ProjPath = os.path.join(drive,path)
    
            # capture the trajectory filename
            fname = row.RunName.split()
            fname = fname[0].replace('\x00','') + ".gps"
            
            # generate the absolute file location
            GPSfile = os.path.join(self.ProjPath,fname)
            GPSfile = os.path.normpath(GPSfile)
    
            if os.path.exists(GPSfile):
                clip = Traj(GPSfile)
                self.clips.append(clip)
            else:
                print "GPS file {0} does not exist".format(GPSfile)
        
        #Create Sensor objects in the Project model       
        sensors = cursor.execute("""SELECT UserRefName, 
                                           ORI2SUP_DX, 
                                           ORI2SUP_DY, 
                                           ORI2SUP_DZ, 
                                           Heading, 
                                           Roll, 
                                           Pitch, 
                                           OriginHeight, 
                                           GPSLag
                                    from C3D""")
        
        DeviceData = cursor2.execute("""SELECT FileName, Path FROM RunDeviceData""")
        
        for device in sensors:
            refname = device.UserRefName.split()
            refname = refname[0].replace('\x00','')
    
            if "Left" in refname:
                for ID in DeviceData:
                    name = ID.FileName.split()
                    name = name[0].replace('\x00','')
                    if "Lt" in name:
                        spath = ID.Path.split()
                        spath = spath[0].replace('\x00','')
                        spath = spath.replace('\\','/')
                        break
                sensorpath = os.path.join(self.ProjPath,spath)
                self.LtLiDAR = sensor(device, sensorpath)
                
            elif "Right" in refname:
                for ID in DeviceData:
                    name = ID.FileName.split()
                    name = name[0].replace('\x00','')
                    if "Rt" in name:
                        spath = ID.Path.split()
                        spath = spath[0].replace('\x00','')
                        spath = spath.replace('\\','/')
                        break
                sensorpath = os.path.join(self.ProjPath,spath)
                self.RtLiDAR = sensor(device, sensorpath)        